/**
* @author als31@aber.ac.uk
*/

#include "capturedevice.h"

openni::Array<openni::DeviceInfo> CaptureDevice::avalaibleDevices;
int CaptureDevice::_activeDevice;


int CaptureDevice::initialize(){

    openni::Status rc = openni::STATUS_OK;

    std::cout << "Initializing OpenNI and Drivers..." << std::endl;
    rc = openni::OpenNI::initialize();

    if(rc != openni::STATUS_OK){
        std::cerr << "OpenNI has failed to Initialize..." << std::endl << "Error: " << openni::OpenNI::getExtendedError() << std::endl;
        openni::OpenNI::shutdown();
        exit(rc);
    }

    openni::OpenNI::enumerateDevices(&avalaibleDevices);
    if(avalaibleDevices.getSize() < 1){
        std::cerr << "OpenNI has not detected any connected Device! "<< openni::OpenNI::getExtendedError() << std::endl;
        openni::OpenNI::shutdown();
        exit(rc);
    }

    std::cout << avalaibleDevices.getSize() << " capture devices detected" << std::endl;
    for(int i = 0 ; i < avalaibleDevices.getSize() ; i++){
        openni::DeviceInfo di = avalaibleDevices[i];
        std::cout << "Device " << i+1 << " : " << di.getUri() << " , " << di.getName() << " , " << di.getVendor() << std::endl;
    }

    return EXIT_SUCCESS;

}

CaptureDevice::CaptureDevice(const char* deviceURI)
{
    _status = INITIALIZING;

    if(avalaibleDevices.getSize() < 1){
        std::cerr << "OpenNI has not detected any connected Device! " << openni::OpenNI::getExtendedError() << std::endl;
        openni::OpenNI::shutdown();
        exit(EXIT_FAILURE);
    }

    int tmp;
    if(!isDeviceDetected(deviceURI, &tmp)){
        std::cerr << "The Device "<< deviceURI << " isn't among the detected devices ! "  << std::endl;
        openni::OpenNI::shutdown();
        exit(EXIT_FAILURE);
    }

    _activeDevice = tmp;

    _colorFrameListener = NULL;
    _depthFrameListener = NULL;

    _status = READY;

}

CaptureDevice::CaptureDevice()
{
    if(avalaibleDevices.getSize() < 1){
        std::cerr << "OpenNI has not detected any connected Device! " << openni::OpenNI::getExtendedError() << std::endl;
        openni::OpenNI::shutdown();
        exit(EXIT_FAILURE);
    }

    openni::DeviceInfo di = avalaibleDevices[0];

    CaptureDevice(di.getUri());
}

CaptureDevice::~CaptureDevice(){
    _depthStream.destroy();
    _colorStream.destroy();
    _captureDevice.close();
    if(_depthFrameListener != NULL)
        delete _depthFrameListener;
    if(_colorFrameListener != NULL)
        delete _colorFrameListener;
    _status = CLOSED;
}


void CaptureDevice::startStream()
{
    openni::DeviceInfo di = avalaibleDevices[_activeDevice];
    std::cout << di.getUri() << std::endl;
    openni::Status rc = _captureDevice.open(di.getUri());
    //openni::Status rc = _captureDevice.open(openni::ANY_DEVICE);
    if (rc != openni::STATUS_OK)
    {
        std::cerr << "Device open failed: " <<  openni::OpenNI::getExtendedError() << std::endl;
        openni::OpenNI::shutdown();
        exit(EXIT_FAILURE);
    }

    rc = _depthStream.create(_captureDevice, openni::SENSOR_DEPTH);
    if (rc == openni::STATUS_OK)
    {
        rc = _depthStream.start();
        if (rc != openni::STATUS_OK)
        {
            std::cerr << "Couldn't start depth stream: " <<  openni::OpenNI::getExtendedError() << std::endl;
            _depthStream.destroy();
        }
    }
    else
    {
        std::cerr << "Couldn't find depth stream: " << openni::OpenNI::getExtendedError() << std::endl;
    }

    rc = _colorStream.create(_captureDevice, openni::SENSOR_COLOR);
    if (rc == openni::STATUS_OK)
    {
        rc = _colorStream.start();
        if (rc != openni::STATUS_OK)
        {
            std::cerr << "Couldn't start color stream: " << openni::OpenNI::getExtendedError() << std::endl;
            _colorStream.destroy();
        }
    }
    else
    {
        std::cerr << "Couldn't find color stream: " << openni::OpenNI::getExtendedError() << std::endl;
    }

    if (!_depthStream.isValid() || !_colorStream.isValid())
    {
        std::cerr <<  "No valid streams. Exiting..." << std::endl;
        openni::OpenNI::shutdown();
        exit(EXIT_FAILURE);
    }else{
        std::cout << "Device Streaming..." << std::endl;
    }

    _status = STREAMING;

    _colorFrameListener = new CaptureDeviceListener();
    _depthFrameListener = new CaptureDeviceListener();

    _depthStream.addNewFrameListener(_depthFrameListener);
    _colorStream.addNewFrameListener(_colorFrameListener);


}

void CaptureDevice::stopStream(){
    std::cout << "Stopping streaming..." << std::endl;

    _depthStream.removeNewFrameListener(_depthFrameListener);
    _colorStream.removeNewFrameListener(_colorFrameListener);

    delete _depthFrameListener;
    delete _colorFrameListener;
    _depthFrameListener = NULL;
    _colorFrameListener = NULL;

    _depthStream.stop();
    _colorStream.stop();
    _captureDevice.close();

    std::cout << "Streaming stopped!" << std::endl;
    _status = READY;
}

int CaptureDevice::isDeviceDetected(const char *deviceURI, int* index)
{
    for(int i = 0 ; i < avalaibleDevices.getSize() ; i++){
        openni::DeviceInfo di = avalaibleDevices[i];
        if( strcmp( di.getUri(), deviceURI) == 0){
            *index = i;
            return 1;
        }
    }

    return 0;
}

void CaptureDeviceListener::onNewFrame(openni::VideoStream &stream){

    const openni::SensorInfo& sensorInfo = stream.getSensorInfo();
    openni::VideoFrameRef frame;

    openni::Status rc = stream.readFrame(&frame);
    if(rc != openni::STATUS_OK){
        std::cout << "Failed reading a frame!" << std::endl;
        return;
    }

    if( sensorInfo.getSensorType() == openni::SENSOR_COLOR){

        if (frame.getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_JPEG &&
                frame.getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888 &&
                frame.getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_YUV422
                )
        {
            std::cerr << "Unexpected frame format from the color sensor" << std::endl;
            return;
        }

        openni::RGB888Pixel* pColor = (openni::RGB888Pixel*)frame.getData();
        emit newFrame(frame.getData(), frame.getHeight(), frame.getWidth(), COLOR_SENSOR);

        int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2;

        printf("Color: [%08llu] %8d\n", (long long)frame.getTimestamp(), pColor[middleIndex]);

    }else if( sensorInfo.getSensorType() == openni::SENSOR_DEPTH ){

        if (frame.getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM &&
                frame.getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_100_UM)
        {
            std::cout << "Unexpected frame format from the depth sensor" << std::endl;
            return;
        }

        openni::DepthPixel* pDepth = (openni::DepthPixel*)frame.getData();
        emit newFrame(frame.getData(), frame.getHeight(), frame.getWidth(), DEPTH_SENSOR);

        int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2;

        printf("Depth:  [%08llu] %8d\n", (long long)frame.getTimestamp(), pDepth[middleIndex]);

    }

}
